// #include <math.h>
// #include <stdio.h>
// #include <stdlib.h>
// #include <string.h>
// #include <inttypes.h>

#include <stdint.h>
// #include <SPI.h>

// #include <util/delay.h>
// #include <avr/pgmspace.h>
// #include <avr/eeprom.h>
// #include <avr/interrupt.h>

#include "./planner.h"
#include "./movement.h"

SemaphoreHandle_t IsSending = NULL;

#define BLOCK_BUFFER_SIZE 16 // maximize block buffer

extern void PlannerInit();
extern void PlaneXY(float X, float Y, float speed);

//The ASCII buffer for receiving from the serial:
#define MAX_CMD_SIZE 48
#define BUFSIZE 8
#define NUM_AXIS 4

bool axis_relative_modes[NUM_AXIS] = {false, false, false, false};
float current_position[NUM_AXIS] = { 0.0, 0.0, 0.0, 0.0 };
const char axis_codes[NUM_AXIS] = {'X', 'Y', 'Z', 'E'};
static float destination[NUM_AXIS] = {  0.0, 0.0, 0.0, 0.0};

static char cmdbuffer[BUFSIZE][MAX_CMD_SIZE];//4x96
static bool fromsd[BUFSIZE];
static int bufindr = 0;
static int bufindw = 0;
static int buflen = 0;
static bool relative_mode = false; 
static char serial_char;
static int serial_count = 0;
static bool comment_mode = false;
static float feedrate = 1500.0, next_feedrate, saved_feedrate;
static char *strchr_pointer = NULL; // just a pointer to find chars in the command string like X, Y, Z, E, etc


float code_value()
{
  return strtof(&cmdbuffer[bufindr][strchr_pointer - cmdbuffer[bufindr] + 1], nullptr);
  
}

// long code_value_long()
// {
//   return (strtol(&cmdbuffer[bufindr][strchr_pointer - cmdbuffer[bufindr] + 1], NULL, 10));
// }

bool code_seen(char code)
{
  strchr_pointer = strchr(cmdbuffer[bufindr], code);
  return (strchr_pointer != NULL);  //Return True if a character was found
}

void enquecommand(const char *cmd)
{
  if(buflen < BUFSIZE)
  {
    //this is dangerous if a mixing of serial and this happens
    strcpy(&(cmdbuffer[bufindw][0]),cmd);
    bufindw= (bufindw + 1)%BUFSIZE;
    buflen += 1;
  }
}

void enquecommand_P(const char *cmd)
{
  if(buflen < BUFSIZE)
  {
    //this is dangerous if a mixing of serial and this happens
    strcpy_P(&(cmdbuffer[bufindw][0]),cmd);
    bufindw= (bufindw + 1)%BUFSIZE;
    buflen += 1;
  }
}

void get_command()
{
  while( Serial.available() > 0  && buflen < BUFSIZE)//when serial port get data and cmdbuffer has space，the cmdbuffer will read G code.
  {
    serial_char = Serial.read();
    if(serial_char == '\n' || serial_char == '\r' || (serial_char == ':' && comment_mode == false) || serial_count >= (MAX_CMD_SIZE - 1) )
    {
      if(!serial_count) //if empty line
      {
        comment_mode = false; //for new command
        return;
      }
      cmdbuffer[bufindw][serial_count] = 0; //terminate string
      if(!comment_mode)
      {
        comment_mode = false; //for new command
        fromsd[bufindw] = false;
        bufindw = (bufindw + 1)%BUFSIZE;
        buflen += 1;
      }
      serial_count = 0; //clear buffer
    }
    else
    {
      if(serial_char == ';') comment_mode = true;
      if(!comment_mode) cmdbuffer[bufindw][serial_count++] = serial_char;
    }
  }

}
void get_coordinates()
{
  bool seen[4]={false,false,false,false};
  for(int8_t i=0; i < NUM_AXIS; i++) 
  {
    if(code_seen(axis_codes[i]))//axis_codes[]={'X','Y','Z','E'}
    {
      destination[i] = (float)code_value() + (axis_relative_modes[i] || relative_mode)*current_position[i];
      seen[i]=true;
    }
    else 
    {
      destination[i] = current_position[i]; //Are these else lines really needed?
    }
  }
  if(code_seen('F')) 
  {
    next_feedrate = code_value();
    if(next_feedrate > 0.0) feedrate = next_feedrate;
  }
}

void prepare_move()
{
    //Plan X and Y
    PlaneXY(destination[0],destination[1],next_feedrate);

    for(int8_t i=0; i < NUM_AXIS; i++) 
    {
        current_position[i] = destination[i];
    }
    //这里设置激光的强度
    XYStartMove();
}
void st_synchronize()
{
//     while( blocks_queued()) {
//     get_command();
//   }
}

void process_commands()
{
  unsigned long codenum; //throw away variable
  char *starpos = NULL;
  if(code_seen('G'))
  {
    switch((int)code_value())
    {
      case 0: // G1 -> G0

        get_coordinates(); // For X Y Z E F
        prepare_move();
        // st_synchronize();        
        // SERIAL_PROTOCOLPGM("ok");   
        break;
      
    // case 1: // G1
     
    //     get_coordinates(); // For X Y Z E F
    //     prepare_move();
    //     st_synchronize();        
         
    //   }
    //   break;
      
    case 4: // G4 dwell
    //   codenum = 0;
    //   if(code_seen('P')) codenum = code_value(); // milliseconds to wait
    //   if(code_seen('S')) codenum = code_value() * 1000; // seconds to wait

    //   st_synchronize();
    //   codenum += millis();  // keep track of when we started waiting
    //   previous_millis_cmd = millis();
    //   while(millis() < codenum) 
    //   {
          
    //     manage_inactivity();
    //   }

    //   SERIAL_PROTOCOLPGM("oMG");   
      break;
    }
  }
  else if(code_seen('M'))
  {

  }
}


void setup()
{
    Serial.begin(115200);
    PlannerInit();
    MovInit();
    IsSending = xSemaphoreCreateBinary();
    xSemaphoreGive(IsSending);

}

void loop()
{
    if(buflen < (BUFSIZE-1))
    {
        get_command();
    }

    if(buflen)
    {
        process_commands();
        buflen = (buflen-1);
        bufindr = (bufindr + 1)%BUFSIZE;
    }
}
